import time
import redis
import minimalmodbus as modbus
import serial


class Wheel(object):
    def __init__(self, _port, _slaveaddress, p_setting_speed, p_setting_direction, p_forward_direction,
                 p_backward_direction, function_code):
        self.slave = modbus.Instrument(port=_port, slaveaddress=_slaveaddress)
        self.slave. debug = False
        self.slave.serial.timeout = 0.1
        self.slave.serial.baudrate = 9600
        self.function_code = function_code
        # self.slave.write_register(0x00B6, 0x0001, 0, self.function_code)
        try:
            self.slave.write_register(0x4657, 1, 0, self.function_code)
        except:
            pass

        self.p_setting_speed = p_setting_speed
        self.p_setting_direction = p_setting_direction
        self.p_forward_direction = p_forward_direction
        self.p_backward_direction = p_backward_direction

        self.current_speed = 0

        self.is_forward = False
        self.is_backward = False

        self.scale = 800

    def forward(self, running_speed=None):
        try:
            if running_speed:
                speed = running_speed
            else:
                speed = self.current_speed + self.scale

            # print(speed)

            if not self.is_forward:
                self.slave.write_register(self.p_setting_speed, speed, 0, self.function_code)
                self.slave.write_register(self.p_setting_direction, self.p_forward_direction, 0, self.function_code)
                self.is_forward = True
            else:
                if self.current_speed == speed:
                    pass
                else:
                    self.current_speed = speed
                    self.slave.write_register(self.p_setting_speed, self.current_speed, 0, self.function_code)
        except:
            print('error')

    def slow_down(self):
        try:
            self.current_speed = self.current_speed - self.scale
            if self.current_speed < 0:
                self.current_speed = 0

            if self.is_forward or self.is_backward:
                self.slave.write_register(self.p_setting_speed, self.current_speed, 0, self.function_code)

            if self.current_speed == 0:
                self.is_forward = False
                self.is_backward = False
        except:
            print('error')

    def stop(self):
        try:
            self.is_forward = False
            self.is_backward = False
            self.current_speed = 0

            self.slave.write_register(self.p_setting_speed, 0, 0, self.function_code)
        except:
            print('error')

    def backward(self, running_speed=None):
        try:
            if running_speed:
                speed = running_speed
            else:
                speed = self.current_speed + self.scale

            if not self.is_backward:
                self.slave.write_register(self.p_setting_speed, speed, 0, self.function_code)
                self.slave.write_register(self.p_setting_direction, self.p_backward_direction, 0, self.function_code)
                self.is_backward = True
            else:
                if self.current_speed == speed:
                    pass
                else:
                    self.current_speed = speed
                    self.slave.write_register(self.p_setting_speed, self.current_speed, 0, self.function_code)
        except:
            print('error')


class WheelTest(object):
    def __init__(self):
        self.current_speed = 0

        self.is_forward = False
        self.is_backward = False

    def forward(self, running_speed=None):
        pass

    def slow_down(self):
        pass

    def stop(self):
        pass

    def backward(self, running_speed=None):
        pass


class Drive(object):
    def __init__(self, left_front, left_rear, right_front, right_rear):
        self.left_front = left_front
        self.left_rear = left_rear
        self.right_front = right_front
        self.right_rear = right_rear

        self.r = redis.Redis(db=0)
        self.new_job = False

        self.task_index = 1
        self.last_oy = 0

    def forward_speed_up(self, speed_left=None, speed_right=None):
        print('forward')
        if self.left_front.is_backward or self.right_front.is_backward:
            self.speed_down()
        else:
            self.left_front.forward(speed_left)
            self.left_rear.forward(speed_left)

            self.right_front.forward(speed_right)
            self.right_rear.forward(speed_right)

    def speed_down(self):
        self.left_front.slow_down()
        self.left_rear.slow_down()

        self.right_front.slow_down()
        self.right_rear.slow_down()

    def go_right(self):
        if self.left_front.current_speed < self.right_front.current_speed:
            if self.left_front.is_forward or self.right_front.is_forward:
                self.left_front.forward()
                self.left_rear.forward()
            else:
                self.left_front.backward()
                self.left_rear.backward()
        else:
            self.right_front.slow_down()
            self.right_rear.slow_down()

    def go_left(self):
        if self.right_front.current_speed < self.left_front.current_speed:
            if self.left_front.is_forward or self.right_front.is_forward:
                self.right_front.forward()
                self.right_rear.forward()
            else:
                self.right_front.backward()
                self.right_rear.backward()
        else:
            self.left_front.slow_down()
            self.left_rear.slow_down()

    def backward_speed_up(self, speed_left=None, speed_right=None):
        if self.left_front.is_forward or self.right_front.is_forward:
            self.speed_down()
        else:
            self.left_front.backward(speed_left)
            self.left_rear.backward(speed_left)

            self.right_front.backward(speed_right)
            self.right_rear.backward(speed_right)

    def rotate_right(self):
        self.left_front.forward()
        self.left_rear.forward()

        self.right_front.backward()
        self.right_rear.backward()

    def rotate_left(self):
        self.left_front.backward()
        self.left_rear.backward()

        self.right_front.forward()
        self.right_rear.forward()

    def rotate_over_fix_speed(self, speed_left=None, speed_right=None):
        if speed_right > 0:
            self.right_front.forward(speed_right)
            self.right_rear.forward(speed_right)
        else:
            self.right_front.backward(-speed_right)
            self.right_rear.backward(-speed_right)

        if speed_left > 0:
            self.left_front.forward(speed_left)
            self.left_rear.forward(speed_left)
        else:
            self.left_front.backward(-speed_left)
            self.left_rear.backward(-speed_left)

    def stop(self):
        self.left_front.stop()
        self.left_rear.stop()

        self.right_front.stop()
        self.right_rear.stop()

    def update_speed(self):
        return self.left_front.current_speed, self.right_front.current_speed


class TestDrive(Drive):
    def __init__(self):
        self.current_speed = 0

    def forward_speed_up(self, speed_left=None, speed_right=None):
        pass

    def speed_down(self):
        pass

    def go_right(self):
        pass

    def go_left(self):
        pass

    def backward_speed_up(self, speed_left=None, speed_right=None):
        pass

    def rotate_right(self):
        pass

    def rotate_left(self):
        pass

    def stop(self):
        pass

    def rotate_over_fix_speed(self,  speed_left=None, speed_right=None):
        print('rotate_over_fix_speed')


if __name__ == '__main__':
    r = redis.Redis(db=0)

    left_front = Wheel('/dev/ttyUSB4', 1, 0x4420, 0x466D, 1, 0, 16)
    # left_front = WheelTest()

    left_rear = Wheel('/dev/ttyUSB2', 1, 0x4420, 0x466D, 1, 0, 16)
    # left_rear = WheelTest()

    right_front = Wheel('/dev/ttyUSB4', 4, 0x4420, 0x466D, 0, 1, 16)
    # right_front =  WheelTest()

    right_rear = Wheel('/dev/ttyUSB2', 2, 0x4420, 0x466D, 0, 1, 16)
    # right_rear = WheelTest()

    #
    # left = Wheel('/dev/ttyUSB1', 1, 0x4420, 0x466D, 1, 0, 16)
    # right = Wheel('/dev/ttyUSB1', 2, 0x4420, 0x466D, 0, 1, 16)

    # left = Wheel('/dev/ttyUSB0', 1, 0x0056, 0x0066, 2, 1, 6)
    # right = Wheel('/dev/ttyUSB0', 2, 0x0056, 0x0066, 1, 2, 6)

    autopilot = Drive(left_front, left_rear, right_front, right_rear)

    # autopilot = TestDrive()

    auto = False
    joystick = False
    tv_control = True
    seed_switch = True

    r.set('auto', 'off')
    r.set('keyboard_input', 0)

    ser = serial.Serial('/dev/ttyUSB3')

    r.set('process_2', 'on')

    while r.get('process_2').decode("utf-8") == 'on':
        left_speed, right_speed = autopilot.update_speed()
        r.set('read_only_left_speed', left_speed)
        r.set('read_only_right_speed', right_speed)

        key = r.get('keyboard_input').decode('ascii')

        if tv_control:
            user_move_timestamp = int(r.get('user_move_timestamp'))

            if time.time() - user_move_timestamp > 3:
                print('stop')
                # autopilot.forward_speed_up(0, 0)
                autopilot.speed_down()

                r.set('forward_left_speed', 0)
                r.set('forward_right_speed', 0)

                # joystick = False
                auto = False

        if key == 'o':
            print('tv_control_off')
            tv_control = False

            r.set('keyboard_input', 0)
        elif key == '/':
            pass

        elif key != '0':
            # tv_control = True
            # auto = False
            joystick = False

            r.set('auto', 'off')

            if key == 'Key.up' or key == 't':  # up
                print('forward')
                autopilot.forward_speed_up()
                tv_control = True
            elif key == 'Key.left' or key == 'f':  # left
                print('left')
                autopilot.go_left()
                tv_control = True
            elif key == 'Key.right' or key == 'h':  # right
                print('right')
                autopilot.go_right()
                tv_control = True
            elif key == 'Key.down' or key == 'g':  # down
                print('back')
                autopilot.backward_speed_up()
                tv_control = True
            elif key == 'Key.space':     # space
                print('stop')
                autopilot.stop()
                auto = False
                tv_control = True
            elif key == 'q':     # q
                print('rotate left')
                autopilot.rotate_left()
                tv_control = True
            elif key == 'e':     # e
                print('rotate right')
                autopilot.rotate_right()
                tv_control = True
            elif key == 'a':     # a
                print('auto')
                auto = True
                tv_control = True
                r.set('auto', 'on')
            elif key == 'r':
                print('remote')
                joystick = True
                tv_control = True
            elif key == 't':
                print('teamviewer control')
                tv_control = True

            elif key == 'd':
                ser.write(b'*OUT8_ON#')
                # tv_control = True
            elif key == 's':
                ser.write(b'*OUT8_OFF#')
                # tv_control = True

            elif key == 'z':
                ser.write(b'*MOTORA2+#')
                # tv_control = True
            elif key == 'x':
                ser.write(b'*MOTORA2=#')
                # tv_control = True
            elif key == 'c':
                ser.write(b'*MOTORA2-#')
                # tv_control = True

            elif key == 'b':
                ser.write(b'*MOTORA1+#')
                # tv_control = True
            elif key == 'n':
                ser.write(b'*MOTORA1=#')
                # tv_control = True
            elif key == 'm':
                ser.write(b'*MOTORA1-#')
                # tv_control = True
            elif key == 'j':
                if seed_switch:
                    seed_switch = False
                    r.set('seed_speed', 0)
                else:
                    seed_switch = True


            r.set('keyboard_input', 0)
        elif auto:
            speed_left = int(r.get('forward_left_speed'))
            speed_right = int(r.get('forward_right_speed'))

            print(speed_left,speed_right)
            if speed_left == 0 or speed_right == 0:
                autopilot.stop()
            elif speed_left*speed_right < 0:
                autopilot.rotate_over_fix_speed(speed_left, speed_right)
            else:
                if speed_left > 0:
                    autopilot.forward_speed_up(speed_left=speed_left, speed_right=speed_right)
                else:
                    autopilot.backward_speed_up(speed_left=-speed_left, speed_right=-speed_right)

            if seed_switch:
                r.set('seed_speed', int((speed_left + speed_right)/2))

        elif joystick:
            speed_left = int(r.get('forward_left_speed'))
            speed_right = int(r.get('forward_right_speed'))

            print("{0} {1}".format(speed_left, speed_right))

            if speed_left == 0 or speed_right == 0:
                autopilot.stop()
            elif speed_left*speed_right < 0:
                autopilot.rotate_over_fix_speed(speed_left, speed_right)
            else:
                if speed_left < 0:
                    autopilot.backward_speed_up(speed_left=-speed_left, speed_right=-speed_right)
                else:
                    autopilot.forward_speed_up(speed_left=speed_left, speed_right=speed_right)

            # time.sleep(0.2)
        else:
            # time.sleep(0.2)
            pass

        time.sleep(0.2)
